/**
 * @file	multi_role.c
 * @author	chipsea
 * @brief	This file contains function that allows user setup tasks
 * @version	0.1
 * @date	2020-11-30
 * @copyright Copyright (c) 2020, CHIPSEA Co., Ltd.
 * @note
 */

/*********************************************************************
 * INCLUDES
 */
#include "sdk_config.h"
#include "bcomdef.h"
#include "OSAL.h"
#include "OSAL_PwrMgr.h"
#include "OSAL_bufmgr.h"
#include "gatt.h"
#include "ll.h"
#include "hci.h"
#include "gap.h"
#include "gapgattserver.h"
#include "gattservapp.h"
#include "multi.h"
#include "gapbondmgr.h"
#include "multi_role.h"
#include "LL_def.h"
#include "ll_common.h"
#include "gatt_uuid.h"
#include "gap_internal.h"

#include "log.h"
//#define LOG(...)

#include "hci.h"
//#include "ll_def.h"

#include "flash.h"

#include "rf_phy_driver.h"
#include "bleuart_service.h"
#include "gap.h"
#include "battservice.h"
#include "hidkbdservice.h"
#include "pwrmgr.h"

/*********************************************************************
 * MACROS
 */

// Length of bd addr as a string
#define B_ADDR_STR_LEN                        15

/*********************************************************************
 * CONSTANTS
 */
// What is the advertising interval when device is discoverable (units of 625us, 64=40ms)
#define DEFAULT_ADVERTISING_INTERVAL          640

//这里是底层的间隔，测了一下对功耗没啥影响
#define DEFAULT_SCAN_WINDOW				      64	//scan window = 88*0.625=55ms
#define DEFAULT_SCAN_INT					  160	//scan interval = 160*0625=100ms
// Scan duration in ms
#define DEFAULT_SCAN_DURATION                 2000	//scan duration: 1000ms

// Discovey mode (limited, general, all)
#define DEFAULT_DISCOVERY_MODE                DEVDISC_MODE_ALL

// TRUE to use active scan
#define DEFAULT_DISCOVERY_ACTIVE_SCAN         TRUE

// TRUE to use white list during discovery
#define DEFAULT_DISCOVERY_WHITE_LIST          FALSE

// TRUE to use high scan duty cycle when creating link
#define DEFAULT_LINK_HIGH_DUTY_CYCLE          TRUE

// TRUE to use white list when creating link
#define DEFAULT_LINK_WHITE_LIST               TRUE

// Default RSSI polling period in ms
#define DEFAULT_RSSI_PERIOD                   1000

// Whether to enable automatic parameter update request when a connection is formed
//#define DEFAULT_ENABLE_UPDATE_REQUEST         FALSE

// Minimum connection interval (units of 1.25ms) if automatic parameter update request is enabled
#define DEFAULT_UPDATE_MIN_CONN_INTERVAL      400

// Maximum connection interval (units of 1.25ms) if automatic parameter update request is enabled
#define DEFAULT_UPDATE_MAX_CONN_INTERVAL      800

// Slave latency to use if automatic parameter update request is enabled
#define DEFAULT_UPDATE_SLAVE_LATENCY          0

// Supervision timeout value (units of 10ms) if automatic parameter update request is enabled
#define DEFAULT_UPDATE_CONN_TIMEOUT           600

// Default passcode
#define DEFAULT_PASSCODE                      19655

// Default GAP pairing mode
#define DEFAULT_PAIRING_MODE                  GAPBOND_PAIRING_MODE_WAIT_FOR_REQ

// Default MITM mode (TRUE to require passcode or OOB when pairing)
#define DEFAULT_MITM_MODE                     TRUE

// Default bonding mode, TRUE to bond
#define DEFAULT_BONDING_MODE                  TRUE

// Default GAP bonding I/O capabilities
#define DEFAULT_IO_CAPABILITIES               GAPBOND_IO_CAP_NO_INPUT_NO_OUTPUT

// Default service discovery timer delay in ms
#define DEFAULT_SVC_DISCOVERY_DELAY           1000

// TRUE to filter discovery results on desired service UUID
#define DEFAULT_DEV_DISC_BY_SVC_UUID          TRUE

#define CONNECT_CNT_TEST						0

// Application states
enum
{
    BLE_STATE_IDLE,
    BLE_STATE_CONNECTING,
    BLE_STATE_CONNECTED,
    BLE_STATE_DISCONNECTING
};

// Discovery states
enum
{
    BLE_DISC_STATE_IDLE,                // Idle
    BLE_DISC_STATE_SVC,                 // Service discovery
    BLE_DISC_STATE_CHAR,                // Characteristic discovery
    BLE_DISC_STATE_RX,                  // discovery TX characteristic
    BLE_DISC_STATE_TX                   // discovery TX characteristic
};
/*********************************************************************
 * TYPEDEFS
 */
/*********************************************************************
 * TYPEDEFS
 */
// add by zhufei.zhang 2018.10.25
// Advertising and Scan Response Data


    

/*********************************************************************
 * GLOBAL VARIABLES
 */
///< Rawpass GATT Profile Service UUID
static CONST uint8 blemulti_ServiceUUID[ATT_BT_UUID_SIZE] =
{ 
	LO_UINT16(BLE_UART_SERV_UUID), HI_UINT16(BLE_UART_SERV_UUID)
};

///< Characteristic tx uuid
static CONST uint8 blemulti_TxCharUUID[ATT_BT_UUID_SIZE] =
{ 
	LO_UINT16(BLE_UART_CHAR1_UUID), HI_UINT16(BLE_UART_CHAR1_UUID)
};
///< Characteristic rx uuid
static CONST uint8 blemulti_RxCharUUID[ATT_BT_UUID_SIZE] =
{ 
	LO_UINT16(BLE_UART_CHAR2_UUID), HI_UINT16(BLE_UART_CHAR2_UUID)
};

uint8 masterLinkUUIDLen = ATT_BT_UUID_SIZE;//主机link过程中UUID的长度，默认2字节
/*********************************************************************
 * EXTERNAL VARIABLES
 */

/*********************************************************************
 * EXTERNAL FUNCTIONS
 */
extern llStatus_t LL_ReadRandomAddr(uint8 *randomAddr);
extern uint8_t LL_GetLinkRole(uint8_t connId);
extern llStatus_t LL_ReadLocalDataLengh(uint16 connId,uint16 *TxOctets,uint16 *RxOctets);

/*********************************************************************
 * LOCAL VARIABLES
 */

// Task ID for internal task/event processing
uint8 simpleBLETaskId;

// GAP GATT Attributes
uint8 simpleBLEDeviceName[GAP_DEVICE_NAME_LEN] = DEVICE_NAME;

// RSSI polling state
//static uint8 simpleBLERssi = FALSE;

/**
 * @brief slave_handle
 * 
 */
slave_handle_t slave_handle[MAX_SLAVE_NUM];

static uint8 scanRspData[18] =
{
    // complete name
    0x11,   // length of this data
    GAP_ADTYPE_128BIT_COMPLETE,
    0xFB,
    0x34,
    0x9B,
    0x5F,
    0x80,
    0x00,
    0x00,
    0x80,
    0x00,
    0x10,
    0x00,
    0x00,
    0xF0,
    0xFF,
    0x00,
    0x00,
};


// advert data for iBeacon
uint8 advertData[GAP_DEVICE_NAME_LEN] =
{
    0x02,   // length of this data
    GAP_ADTYPE_FLAGS,
    GAP_ADTYPE_FLAGS_GENERAL | GAP_ADTYPE_FLAGS_BREDR_NOT_SUPPORTED,
    0x07,
    0xFF,
    0x01,0x02,0x03,0x04,0x05,0x06//mac地址
    /*0x0D,
    GAP_ADTYPE_LOCAL_NAME_COMPLETE,
    'C',
    'h',
    'i',
    'p',
    's',
    'e',
    'a',
    'M',
    'u',
    'l',
    't',
    'i'*/
};

/*********************************************************************
 * LOCAL FUNCTIONS
 */
static void multiRoleAppProcessGATTMsg(gattMsgEvent_t *pMsg);
static void multiRoleAppEventCB(gapCentralRoleEvent_t *pEvent);
#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
static void simpleBLECentralPasscodeCB(uint8 *deviceAddr, uint16 connectionHandle,
                                       uint8 uiInputs, uint8 uiOutputs);
static void simpleBLECentralPairStateCB(uint16 connHandle, uint8 state, uint8 status);
#endif
static void multiRoleApp_ProcessOSALMsg(osal_event_hdr_t *pMsg);

bool simpleBLEFindSvcUuid(uint16 uuid, uint8 *pData, uint8 dataLen);
char *bdAddr2Str(uint8 *pAddr);

static void simpleBLECentralStartConnecting(void);
static bStatus_t multiRoleAppStartDiscoveryServiceByUUID(uint8_t masterIndex);
static bStatus_t multiRoleAppStartDiscoveryAllCharacteristic(uint8_t masterIndex);
static bStatus_t multiRoleAppStartDiscoveryAllCharacteristicDesc(uint8_t masterIndex);
static bStatus_t multiRoleAppEnableNotification(uint8_t masterIndex);
static void multiRoleGattServMsgCB(gattMsgEvent_t*pMsg);

/*********************************************************************
 * PROFILE CALLBACKS
 */

// GAP Role Callbacks
static const gapMultiRolesCBs_t simpleBLERoleCB =
{
    NULL,
    NULL,                     // RSSI callback
    multiRoleAppEventCB       // Event callback
};

#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
// Bond Manager Callbacks
static const gapBondCBs_t simpleBLEBondCB =
{
	simpleBLECentralPasscodeCB,
	simpleBLECentralPairStateCB
};
#endif



LinkInitParam_t linkInitParam[MAX_SLAVE_NUM];		//only used in master role
PeerConnectionInfo_t slaveDevInfo[MAX_SLAVE_NUM];//从机设备信息
uint8 g_slaveMaxNum = MAX_SLAVE_NUM;
uint8 g_slaverConnectNum;

PeerConnectionInfo_t masterDevInfo[MAX_MASTER_NUM];//主机设备信息
uint8 g_masterMaxNum = MAX_MASTER_NUM;
uint8 g_masterConnectNum;

uint8 scanDataAddr[DEFAULT_MAX_SCAN_RES][B_ADDR_LEN];//扫描到的地址数据存在此处
extern uint8 gapRole_TaskID;
extern gattAttribute_t bleuart_ProfileAttrTbl[];

/*********************************************************************
 * PUBLIC FUNCTIONS
 */

void get_ble_mac(uint8 *mac)
{
    uint8  flash_mac[8] = {0};
    uint32 check;
    
    if(mac == NULL)
    {
        return;
    }

    HalFlashRead(0x1000,flash_mac,8);
    check = flash_mac[0] + flash_mac[1] + flash_mac[2] + flash_mac[3] + flash_mac[4] + flash_mac[5];
    
    if(check == 0xFF*6)
    {
        osal_memset(mac, 0xFF, 6);
    }
    else
    {
        mac[0] = flash_mac[5];
        mac[1] = flash_mac[4];
        mac[2] = flash_mac[3];
        mac[3] = flash_mac[2];
        mac[4] = flash_mac[1];
        mac[5] = flash_mac[0];
    }
}


//设置主机数量
void ble_set_master_num(uint8_t num)
{
    if(num > MAX_MASTER_NUM)//
    {
        return ;
    }
    g_masterMaxNum = num;
}

//设置从机数量
void ble_set_slave_num(uint8_t num)
{
    if(num > MAX_SLAVE_NUM)
    {
        return;
    }
    g_slaveMaxNum = num;
}
//获取当前允许主机数量
uint8_t ble_get_master_num(void)
{
    return g_masterMaxNum;
}
//获取当前允许从机数量
uint8_t ble_get_slave_num(void)
{
    return g_slaveMaxNum;
}

//获取主机连接数量
uint8_t ble_get_master_link_num(void)
{
    return g_masterConnectNum;
}
//获取从机连接数量
uint8_t ble_get_slave_link_num(void)
{
    return g_slaverConnectNum;
}

/*********************************************************************
 * @fn      SimpleBLECentral_Init
 *
 * @brief   Initialization function for the Simple BLE Central App Task.
 *          This is called during initialization and should contain
 *          any application specific initialization (ie. hardware
 *          initialization/setup, table initialization, power up
 *          notification).
 *
 * @param   task_id - the ID assigned by OSAL.  This ID should be
 *                    used to send messages and set timers.
 *
 * @return  none
 */
extern  uint8 g_conn_taskID;
extern  uint16 g_conn_taskEvent;
void multiRoleApp_Init(uint8 task_id)
{
//    uint8 buf[6];
//    uint8 mac[12];
    uint8_t i;
    simpleBLETaskId = task_id;
    for(i=0;i<g_slaveMaxNum;i++)
    {
        slaveDevInfo[i].ConnectionHandle = 0xFF;
        //slaveDevInfo[i].AutoConnection = 0x00;//默认不允许自动重连接
    }
    for(i=0;i<g_masterMaxNum;i++)
    {
        masterDevInfo[i].ConnectionHandle = 0xFF;
    }
    llInitFeatureSetDLE(1);
    llInitFeatureSet2MPHY(1);
    llInitFeatureSetCSA2(1);

    // Setup the GAP Peripheral Role Profile
    {
        // device starts advertising upon initialization
  
        //uint8 enable_update_request = DEFAULT_ENABLE_UPDATE_REQUEST;
        uint8 advChnMap = GAP_ADVCHAN_37 | GAP_ADVCHAN_38 | GAP_ADVCHAN_39; 
          
        // By setting this to zero, the device will go into the waiting state after
        // being discoverable for 30.72 second, and will not being advertising again
        // until the enabler is set back to TRUE
        uint16 gapRole_AdvertOffTime = 0;//0;			// temp set 1
        
        uint8 peerPublicAddr[] =
        {
              0x01,
              0x02,
              0x03,
              0x04,
              0x05,
              0x06
        };
        

        GAPMultiRole_SetParameter(GAPROLE_ADV_DIRECT_ADDR, sizeof(peerPublicAddr), peerPublicAddr, 0);
        // set adv channel map
        GAPMultiRole_SetParameter(GAPROLE_ADV_CHANNEL_MAP, sizeof(uint8), &advChnMap, 0); 	   
        
        //GAPMultiRole_SetParameter(GAPROLE_BD_ADDR, B_ADDR_LEN, user_data.mac,0);
        // Set the GAP Role Parameters

        GAPMultiRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime, 0);

    }
    // Set advertising interval
    GAP_SetParamValue(TGAP_LIM_DISC_ADV_INT_MIN, DEFAULT_ADVERTISING_INTERVAL);//设置广播间隔（PS：这里是设置从机的广播）
    GAP_SetParamValue(TGAP_LIM_DISC_ADV_INT_MAX, DEFAULT_ADVERTISING_INTERVAL);
    GAP_SetParamValue(TGAP_GEN_DISC_ADV_INT_MIN, DEFAULT_ADVERTISING_INTERVAL);
    GAP_SetParamValue(TGAP_GEN_DISC_ADV_INT_MAX, DEFAULT_ADVERTISING_INTERVAL);
    GAP_SetParamValue(TGAP_CONN_ADV_INT_MIN, DEFAULT_ADVERTISING_INTERVAL);
    GAP_SetParamValue(TGAP_CONN_ADV_INT_MAX, DEFAULT_ADVERTISING_INTERVAL);

    // Setup Central Profile
    {
        uint8 scanRes = DEFAULT_MAX_SCAN_RES;
        GAPMultiRole_SetParameter(GAPROLE_MAX_SCAN_RES, sizeof(uint8), &scanRes, 0);//GAPROLE_MAX_SCAN_RES是设置最大接收发现扫描结果数，默认是9,0是无限
    }

    // Set scan duration parameter

    GAP_SetParamValue(TGAP_GEN_DISC_SCAN_INT,DEFAULT_SCAN_INT);//设置扫描时间相关参数
	GAP_SetParamValue(TGAP_GEN_DISC_SCAN_WIND, DEFAULT_SCAN_WINDOW);
	GAP_SetParamValue(TGAP_GEN_DISC_SCAN, DEFAULT_SCAN_DURATION);
	GAP_SetParamValue(TGAP_LIM_DISC_SCAN_INT, DEFAULT_SCAN_INT);
	GAP_SetParamValue(TGAP_LIM_DISC_SCAN_WIND, DEFAULT_SCAN_WINDOW);
	GAP_SetParamValue(TGAP_LIM_DISC_SCAN, DEFAULT_SCAN_DURATION);
	
	//Set connect init parameter
    GAP_SetParamValue(TGAP_CONN_HIGH_SCAN_INT, DEFAULT_SCAN_WINDOW);//设置扫描间隔
    GAP_SetParamValue(TGAP_CONN_HIGH_SCAN_WIND, DEFAULT_SCAN_WINDOW);//设置扫描窗口间隔

    //建议连接间隔
    //GAPMultiRole_SetParameter(GAPROLE_MIN_CONN_INTERVAL, 2, (uint8_t *)&connin_table[user_data.connin], 0);
    //GAPMultiRole_SetParameter(GAPROLE_MAX_CONN_INTERVAL, 2, (uint8_t *)&connin_table[user_data.connin], 0);

    // Set master connection parameter
    GAP_SetParamValue(TGAP_CONN_EST_INT_MIN, 40);        // 40 * 1.25ms = 100ms 设置连接间隔
    GAP_SetParamValue(TGAP_CONN_EST_INT_MAX, 40);
    GAP_SetParamValue(TGAP_CONN_EST_SUPERV_TIMEOUT, 500);     // 500 * 10ms = 5000ms 设置连接超时

#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
	// Setup the GAP Bond Manager
	{
		uint32 passkey = DEFAULT_PASSCODE;
		uint8 pairMode = GAPBOND_PAIRING_MODE_WAIT_FOR_REQ;//GAPBOND_PAIRING_MODE_INITIATE;//DEFAULT_PAIRING_MODE;    // GAPBOND_PAIRING_MODE_NO_PAIRING
		uint8 mitm = DEFAULT_MITM_MODE;
		uint8 ioCap = DEFAULT_IO_CAPABILITIES;
		uint8 bonding = DEFAULT_BONDING_MODE;
		GAPBondMgr_SetParameter(GAPBOND_DEFAULT_PASSCODE, sizeof(uint32), &passkey);
		GAPBondMgr_SetParameter(GAPBOND_PAIRING_MODE, sizeof(uint8), &pairMode);
		GAPBondMgr_SetParameter(GAPBOND_MITM_PROTECTION, sizeof(uint8), &mitm);
		GAPBondMgr_SetParameter(GAPBOND_IO_CAPABILITIES, sizeof(uint8), &ioCap);
		GAPBondMgr_SetParameter(GAPBOND_BONDING_ENABLED, sizeof(uint8), &bonding);
	}
#endif

    // Initialize GATT Client
    VOID GATT_InitClient();

    // Register to receive incoming ATT Indications/Notifications
    GATT_RegisterForInd(simpleBLETaskId);
    
    // Register callback for GATT Service message callback
    gattServApp_RegisterCB(multiRoleGattServMsgCB);


    // Initialize GATT attributes
    GGS_AddService(GATT_ALL_SERVICES);           // GAP
    GATTServApp_AddService(GATT_ALL_SERVICES);   // GATT attributes
    bleuart_AddService(NULL);
    
#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
    Batt_AddService();
    HidKbd_AddService();
#endif

	// Setup a delayed profile startup
	osal_set_event(simpleBLETaskId, START_DEVICE_EVT);
    
    ble_task_set(USER_TASK_START, START_ADVERT_EVT, 500);
//    osal_start_reload_timer(simpleBLETaskId, OTHER_EVT, 1000);
}

uint8_t findConnectIndexByConnectionHandle(uint8_t role, uint8_t connectionHandle)
{
    uint8_t i;
    uint8_t ret = 0xFF;

    if(role == LL_ROLE_MASTER)
    {
        for(i=0; i<g_slaveMaxNum; i++)
        {
            if(connectionHandle ==  slaveDevInfo[i].ConnectionHandle)
            {
                ret = i;
            }
        }
    }
    else if(role == LL_ROLE_SLAVE)
    {
        for(i=0; i<g_masterMaxNum; i++)
        {
            if(connectionHandle ==  masterDevInfo[i].ConnectionHandle)
            {
                ret = i;
            }
        }
    }

    return ret;
}

uint8_t findConnectIndexByAddr(uint8_t role, uint8_t *addr)
{
    uint8_t i;
    uint8_t ret = 0xFF;

    if(role == LL_ROLE_MASTER)
    {
        for(i=0; i<g_slaveMaxNum; i++)
        {
            if(osal_memcmp(addr, slaveDevInfo[i].PeerAddr, B_ADDR_LEN))
            {
                ret = i;
                break;
            }
        }
    }
    else if(role == LL_ROLE_SLAVE)
    {
        for(i=0; i<g_masterMaxNum; i++)
        {
            if(osal_memcmp(addr, masterDevInfo[i].PeerAddr, B_ADDR_LEN))
            {
                ret = i;
                break;
            }
        }
    }

    return ret;
}

uint8_t findConnectionHandleByAddr(uint8_t role, uint8_t *addr)
{
    uint8_t i;
    uint8_t ret = 0xFF;

    if(role == LL_ROLE_MASTER)
    {
        for(i=0; i<g_slaveMaxNum; i++)
        {
            if(osal_memcmp(addr, slaveDevInfo[i].PeerAddr, B_ADDR_LEN))
            {
                ret = slaveDevInfo[i].ConnectionHandle;
            }
        }
    }
    else if(role == LL_ROLE_SLAVE)
    {
        for(i=0; i<g_masterMaxNum; i++)
        {
            if(osal_memcmp(addr, masterDevInfo[i].PeerAddr, B_ADDR_LEN))
            {
                ret = masterDevInfo[i].ConnectionHandle;
            }
        }
    }

    return ret;
}

uint8_t writeConnectIndexByAddr(uint8_t *addr)
{
    //uint8_t ret = 0xFF;
    uint8_t temp_null_mac[6] = {0};//空地址  用来判断地址是否为空
    static uint8_t temp_channel = 0;
    if(temp_channel > MAX_SLAVE_NUM-1)
    {
        temp_channel = 0;
    }
    //osal_memcmp(addr,slaveDevInfo[channel].PeerAddr,6);
    LOG("addr :");
    LOG_DUMP_BYTE(addr,6);
    LOG("slaveDevInfo:\n");
    for(int i = 0 ; i < g_slaveMaxNum ; i ++)
    {
        LOG("channel : %d-add:\n",i);
        LOG_DUMP_BYTE(slaveDevInfo[i].PeerAddr,6);
    }
    LOG("\r\n");

    for(int i = 0 ; i < g_slaveMaxNum ; i ++)//先检查写的地址，有没有和以前重复
    {
        //LOG("osal_memcmp(addr,slaveDevInfo[%d].PeerAddr,6)= %d -\n",i,osal_memcmp(addr,slaveDevInfo[i].PeerAddr,6));
        if(osal_memcmp(addr,slaveDevInfo[i].PeerAddr,6)&&(slaveDevInfo[i].ConnectionHandle == 0xff))
        {
            LOG(" one i %d\n",i);
            //再写一遍
            osal_memcpy(slaveDevInfo[i].PeerAddr, addr, B_ADDR_LEN);
            slaveDevInfo[i].PeerAddrType = LL_DEV_ADDR_TYPE_PUBLIC;
            slaveDevInfo[i].ConnectionHandle = 0xFF;
            slaveDevInfo[i].ConnectionState = 0;
            slaveDevInfo[i].ConnectionActive = 0;
            //有写过直接把通道号返回出去
            return i;
        }
        else if(osal_memcmp(addr,slaveDevInfo[i].PeerAddr,6)&&(slaveDevInfo[i].ConnectionHandle != 0xff))
        {
            LOG(" one else i %d\n",i);
            return i;
        }
    }

    for(int i = 0 ; i < g_slaveMaxNum; i ++)//再判断该通道是否为空，是否写过地址
    {
        //LOG("osal_memcmp(temp_null_mac,slaveDevInfo[%d].PeerAddr,6) %d\n",i,osal_memcmp(temp_null_mac,slaveDevInfo[i].PeerAddr,6));
        if(osal_memcmp(temp_null_mac,slaveDevInfo[i].PeerAddr,6))//地址为空，可以写入
        {
            osal_memcpy(slaveDevInfo[i].PeerAddr, addr, B_ADDR_LEN);    
            slaveDevInfo[i].PeerAddrType = LL_DEV_ADDR_TYPE_PUBLIC;
            slaveDevInfo[i].ConnectionHandle = 0xFF;
            slaveDevInfo[i].ConnectionState = 0;
            slaveDevInfo[i].ConnectionActive = 0;
            LOG(" 2 i %d\n",i);
            return i;
        }
    }

    for(int i = 0 ; i < g_slaveMaxNum; i ++)//如果没有空闲通道，判断当前通道是否连接着
    {
        if(slaveDevInfo[i].ConnectionHandle == 0xff)//该通道无设备使用
        {
            osal_memcpy(slaveDevInfo[i].PeerAddr, addr, B_ADDR_LEN);    
            slaveDevInfo[i].PeerAddrType = LL_DEV_ADDR_TYPE_PUBLIC;
            slaveDevInfo[i].ConnectionHandle = 0xFF;
            slaveDevInfo[i].ConnectionState = 0;
            slaveDevInfo[i].ConnectionActive = 0;
            LOG(" 3 i %d\n",i);
            return i;
        }
    }
    //先断开原来的设备
    HCI_LE_RemoveWhiteListCmd(LL_WHITELIST_DEV_TYPE_SLAVE,LL_DEV_ADDR_TYPE_PUBLIC,slaveDevInfo[temp_channel].PeerAddr);//实测不删白名单也不会回连，不知道为什么，保险起见还是删了
    GAPMultiRole_TerminateConnection(slaveDevInfo[temp_channel].ConnectionHandle);
    //然后替换新的设备进来
    osal_memcpy(slaveDevInfo[temp_channel].PeerAddr, addr, B_ADDR_LEN);    
    slaveDevInfo[temp_channel].PeerAddrType = LL_DEV_ADDR_TYPE_PUBLIC;
    slaveDevInfo[temp_channel].ConnectionHandle = 0xFF;
    slaveDevInfo[temp_channel].ConnectionState = 0;
    slaveDevInfo[temp_channel].ConnectionActive = 0;

    LOG(" 4 temp_channel %d\n",temp_channel);

    return temp_channel++;

}
//根据地址获取角色
uint8_t getConnectRoleByAddr( uint8_t *addr)
{
    uint8_t i;
    uint8_t ret = 0xFF;


    for(i=0; i<g_masterMaxNum; i++)
    {
        if(osal_memcmp(addr, masterDevInfo[i].PeerAddr, B_ADDR_LEN))
        {
            ret = LL_ROLE_SLAVE;
            LOG("LL_ROLE_SLAVE\n");
            break;
        }
    }

    for(i=0; i<g_slaveMaxNum; i++)
    {
        if(osal_memcmp(addr, slaveDevInfo[i].PeerAddr, B_ADDR_LEN))
        {
            ret = LL_ROLE_MASTER;
            LOG("LL_ROLE_MASTER\n");
            break;
        }
    }
    return ret;
}

bStatus_t linkInitProcess(uint16_t masterIndex)
{
    bStatus_t status = SUCCESS;
    //LOG("LinkInit:%d,%d\n",masterIndex, linkInitParam[masterIndex].link_status);
    switch(linkInitParam[masterIndex].link_status)
    {
        case LINK_STATUS_READ_VERSION:
            status = HCI_ReadRemoteVersionInfoCmd(slaveDevInfo[masterIndex].ConnectionHandle);
        break;
        
        case LINK_STATUS_SET_DATALEN:
            status = HCI_LE_SetDataLengthCmd(slaveDevInfo[masterIndex].ConnectionHandle, LL_PDU_LENGTH_SUPPORTED_MAX_TX_OCTECTS, LL_PDU_LENGTH_SUPPORTED_MAX_TX_TIME);
        break;
        
        case LINK_STATUS_SET_PHYMODE:
            status = HCI_LE_SetPhyMode(slaveDevInfo[masterIndex].ConnectionHandle, 0, LE_2M_PHY, LE_2M_PHY, 0);
        break;
        
        case LINK_STATUS_EXCHANGE_MTU:
        {
            attExchangeMTUReq_t pReq;
            pReq.clientRxMTU = ATT_MAX_MTU_SIZE;
            status = GATT_ExchangeMTU(slaveDevInfo[masterIndex].ConnectionHandle, &pReq, simpleBLETaskId);
        }
        break;
        
        case LINK_STATUS_DISCOVER_SERVICE_BY_UUID:
            status = multiRoleAppStartDiscoveryServiceByUUID(masterIndex);
        break;
        
        case LINK_STATUS_DISCOVER_ALL_CHARACTER:
            status = multiRoleAppStartDiscoveryAllCharacteristic(masterIndex);
            break;
        
        case LINK_STATUS_DISCOVER_ALL_CHARACTER_DESC:
            status = multiRoleAppStartDiscoveryAllCharacteristicDesc(masterIndex);
            break;
        
        case LINK_STATUS_ENABLE_NOTIFY:
            status = multiRoleAppEnableNotification(masterIndex);
        break;
    }
    return status;
}

void init_peer_address(void)
{
    for(uint8_t i=0; i<g_slaveMaxNum; i++)
    {
#if 1//(DEFAULT_MODE_TYPE != MODE_TYPE_INVALID)
        if(i == 0)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE5;
            slaveDevInfo[i].PeerAddr[0] = 0xC8;
        }
		else if(i == 1)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE6;
            slaveDevInfo[i].PeerAddr[0] = 0xA0;
        }
        else if(i == 2)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE6;
            slaveDevInfo[i].PeerAddr[0] = 0xAD;
        }
        else if(i == 3)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE5;
            slaveDevInfo[i].PeerAddr[0] = 0xA9;
        }
        else if(i == 4)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE5;
            slaveDevInfo[i].PeerAddr[0] = 0xD1;
        }
        else if(i == 5)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE6;
            slaveDevInfo[i].PeerAddr[0] = 0xC0;
        }
        else if(i == 6)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE2;
            slaveDevInfo[i].PeerAddr[0] = 0x38;
        }
        else if(i == 7)
        {
            slaveDevInfo[i].PeerAddr[5] = 0x50;
            slaveDevInfo[i].PeerAddr[4] = 0xFB;
            slaveDevInfo[i].PeerAddr[3] = 0x19;
            slaveDevInfo[i].PeerAddr[2] = 0xC8;
            slaveDevInfo[i].PeerAddr[1] = 0xE6;
            slaveDevInfo[i].PeerAddr[0] = 0xAB;
        }
		
        slaveDevInfo[i].PeerAddrType = LL_DEV_ADDR_TYPE_UNKNOWN;
		HCI_LE_AddWhiteListCmd(LL_WHITELIST_DEV_TYPE_SLAVE, slaveDevInfo[i].PeerAddrType,&slaveDevInfo[i].PeerAddr[0]);
		LOG("init_peer\n");
#endif		
		slaveDevInfo[i].ConnectionHandle = 0xFF;
		slaveDevInfo[i].ConnectionState = 0;
        slaveDevInfo[i].ConnectionActive = 0;
		
	}
    for(uint8_t i=0;i<g_masterMaxNum;i++)
    {
        if(masterDevInfo[i].ConnectionState == 0)
        {
            masterDevInfo[i].ConnectionHandle = 0xFF;
        }
	}
}

/*********************************************************************
 * @fn      SimpleBLECentral_ProcessEvent
 *
 * @brief   Simple BLE Central Application Task event processor.  This function
 *          is called to process all events for the task.  Events
 *          include timers, messages and any other user defined events.
 *
 * @param   task_id  - The OSAL assigned task ID.
 * @param   events - events to process.  This is a bit map and can
 *                   contain more than one event.
 *
 * @return  events not processed
 */
uint16 multiRoleApp_ProcessEvent(uint8 task_id, uint16 events)
{

    VOID task_id; // OSAL required parameter that isn't used in this function
    uint8_t   numConns = 0;

    //LOG("multiRoleApp_ProcessEvent : %d\n", events);

    if(events & SYS_EVENT_MSG)
    {
        uint8 *pMsg;

        if((pMsg = osal_msg_receive(simpleBLETaskId)) != NULL)
        {
            multiRoleApp_ProcessOSALMsg((osal_event_hdr_t *)pMsg);
            // Release the OSAL message
            VOID osal_msg_deallocate(pMsg);
        }

        // return unprocessed events
        return (events ^ SYS_EVENT_MSG);
    }

    if(events & START_DEVICE_EVT)
    {
        // Start the Device
        VOID GAPMultiRole_StartDevice((gapMultiRolesCBs_t *)&simpleBLERoleCB, &numConns);

    #if (DEFAULT_GAPBOND_MGR_ENABLE==1)
        // Register with bond manager after starting device
        GAPBondMgr_Register((gapBondCBs_t *) &simpleBLEBondCB);
    #endif
        return (events ^ START_DEVICE_EVT);
    }
    
    if(events & START_LINK_STATUS_EVT)
    {
        uint8_t index;
        bStatus_t status;
        for(index=0; index<g_slaveMaxNum; index++)
        {
            if(linkInitParam[index].link_cmd)
            {
                status = linkInitProcess(index);
                if(status == SUCCESS)
                {
                    linkInitParam[index].link_cmd = 0;
                }
            }
        }
        return (events ^ START_LINK_STATUS_EVT);
    }
    
    if(events & START_MASTER_CONN_EVT)
    {

        simpleBLECentralStartConnecting();
        return (events ^ START_MASTER_CONN_EVT);
    }

	if(events & MASTER_CONN_TIMEOUT_EVT)
	{
		GAPMultiRole_TerminateConnection(GAP_CONNHANDLE_INIT);
		//连接失败，重新开启连接
		LOG("Establish Link Time Out.\r\n");
        if(g_slaverConnectNum < g_slaveMaxNum)
		{
			ble_task_set(USER_TASK_START, START_MASTER_CONN_EVT, 500);
		}
		return (events ^ MASTER_CONN_TIMEOUT_EVT);
	}
    
    if(events & START_SCAN_EVT)
	{
		uint8_t scanState;
        
		GAPMultiRole_GetParameter(GAPROLE_SCAN_STATE, &scanState, 0);

		if(scanState == GAPROLE_SCAN_OFF_STATE)
		{
			LOG("GATT_DISCOVERY_EVT\r\n");
            
			GAPMultiRole_StartDiscovery(DEFAULT_DISCOVERY_MODE,
			                            DEFAULT_DISCOVERY_ACTIVE_SCAN,      // passive scan
			                            DEFAULT_DISCOVERY_WHITE_LIST);
		}
//        ble_task_set(USER_TASK_START, START_SCAN_EVT, 5000);
		return (events ^ START_SCAN_EVT);
	}
    
    if(events & START_ADVERT_EVT)
    {
        uint8 offset = 3;
        uint8 initial_advertising_enable;
        MultiRole_EncodeAdvDeviceAddr(advertData,&offset,B_MAX_ADV_LEN);
        MultiRole_EncodeAdvDeviceName(advertData, &offset, B_MAX_ADV_LEN, simpleBLEDeviceName, osal_strlen((char *)simpleBLEDeviceName));
        GAPMultiRole_SetParameter( GAPROLE_SCAN_RSP_DATA, 18, scanRspData, 0);
        GGS_SetParameter(GGS_DEVICE_NAME_ATT, osal_strlen((char *)simpleBLEDeviceName), (uint8 *)simpleBLEDeviceName);//设置GATT服务下的查询蓝牙名称
        GAPMultiRole_SetParameter( GAPROLE_ADVERT_DATA, osal_strlen((char *)advertData), advertData, 0);
        //rf_phy_set_txPower(g_rfPhyTxPower);
        if(g_masterConnectNum < g_masterMaxNum)
        {
            //开启广播
            LOG("enable adv\r\n");
            initial_advertising_enable = TRUE;
            GAPMultiRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable, 0);
        }

		//开启连接
        init_peer_address();
		ble_task_set(USER_TASK_START, START_MASTER_CONN_EVT, NULL);
//        ble_task_set(USER_TASK_START, START_SCAN_EVT, NULL);
        return ( events ^ START_ADVERT_EVT );
    }
#if CONNECT_CNT_TEST
    if(events & START_DISC_TEST_EVT)
    {
        if(slaveDevInfo[0].ConnectionState != 0)
        {
            GAPMultiRole_TerminateConnection(slaveDevInfo[0].ConnectionHandle);
        }
        return ( events ^ START_DISC_TEST_EVT );
    }
#endif
    
    if(events & OTHER_EVT)
	{
        
		return ( events ^ OTHER_EVT );
	}
    
	// Discard unknown events
	return 0;
}

/*********************************************************************
 * @fn      simpleBLECentral_ProcessOSALMsg
 *
 * @brief   Process an incoming task message.
 *
 * @param   pMsg - message to process
 *
 * @return  none
 */
static void multiRoleApp_ProcessOSALMsg(osal_event_hdr_t *pMsg)
{
    switch(pMsg->event)
    {
        case GATT_MSG_EVENT:
            
            multiRoleAppProcessGATTMsg((gattMsgEvent_t *) pMsg);
            break;
    }
}

static void multiRoleGattServMsgCB(gattMsgEvent_t*pMsg)
{
    switch(pMsg->method)
    {
        case ATT_EXCHANGE_MTU_REQ:
            LOG( "Slave MTU Size: %d\r\n", ATT_GetCurrentMTUSize(pMsg->connHandle));//attMtuInfo[pMsg->connHandle].localMTU);
            break;
        default:
            break;
    }
}

/*********************************************************************
 * @fn      multiRoleAppProcessGATTMsg
 *
 * @brief   Process GATT messages
 *
 * @return  none
 */
static void multiRoleAppProcessGATTMsg(gattMsgEvent_t *pMsg)
{
	uint8 index;
	if((pMsg->method == ATT_EXCHANGE_MTU_RSP) ||
      ( (pMsg->method == ATT_ERROR_RSP) &&
        (pMsg->msg.errorRsp.reqOpcode == ATT_EXCHANGE_MTU_REQ) ))
	{
		if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;
            //      
            LOG( "Master EXCHANGE_MTU Error %d,%d\r\n", pMsg->connHandle, status );
            GAPMultiRole_TerminateConnection(pMsg->connHandle);
        }
        else
        {
            // After a successful exchange mtu, display the mtu_size
            index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
            if(index < g_slaveMaxNum)
            {
                LOG( "MTU Size: %d,%d\r\n", index, ATT_GetCurrentMTUSize(pMsg->connHandle));//attMtuInfo[pMsg->connHandle].localMTU);
                if(slave_handle[index].ccc)
                {
                    linkInitParam[index].link_status = LINK_STATUS_ENABLE_NOTIFY;
                }
                else
                {
                    linkInitParam[index].link_status = LINK_STATUS_DISCOVER_SERVICE_BY_UUID;
                }
                linkInitParam[index].link_cmd = 1;
                ble_task_set(USER_TASK_START, START_LINK_STATUS_EVT, NULL);
            }
        }
	}
    else if( (pMsg->method == ATT_READ_RSP) ||
      ( (pMsg->method == ATT_ERROR_RSP) &&
        (pMsg->msg.errorRsp.reqOpcode == ATT_READ_REQ) ) )
    {
        if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;
            //      
            LOG( "Read Error %d\r\n", status );
        }
        else
        {
            // After a successful read, display the read value
            uint8 valueRead = pMsg->msg.readRsp.value[0];

            LOG( "Read rsp: %d\r\n", valueRead);
        }
    }
    else if( (pMsg->method == ATT_WRITE_RSP) ||
           ( (pMsg->method == ATT_ERROR_RSP) &&
             (pMsg->msg.errorRsp.reqOpcode == ATT_WRITE_REQ) ) )
    {
        if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;

            LOG( "Write Error: %d\r\n", status );
        }
        else
        {
            index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
            if(index < g_slaveMaxNum)
            {
                if(linkInitParam[index].link_status == LINK_STATUS_ENABLE_NOTIFY)
                {
                    //连接初始化完成
//					user_uart_task_set(USER_CMD_LINK_REP, (index|0x80));
                    LOG("Link init OK:%d\n",index);
                    linkInitParam[index].link_status = LINK_STATUS_FINISH;
                    slaveDevInfo[index].ConnectionActive = 1;
                    LOG("LocalMtuSize=%d\n",ATT_GetCurrentMTUSize(slaveDevInfo[index].ConnectionHandle));
                }
            }
            LOG( "Write sent\r\n");
        }
    }
    else if( pMsg->method == ATT_HANDLE_VALUE_NOTI  ||
           ( (pMsg->method == ATT_ERROR_RSP) &&
             (pMsg->msg.errorRsp.reqOpcode == ATT_HANDLE_VALUE_NOTI) ) )
    {
		index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
        if( pMsg->method == ATT_ERROR_RSP || 
            pMsg->msg.handleValueNoti.len > ATT_GetCurrentMTUSize(pMsg->connHandle)-3 )
        {
            uint8 status = pMsg->msg.errorRsp.errCode;
            LOG( "Ntf Error: %d,%d\r\n", index, status );
        }
        else
        {
            if(index < g_slaveMaxNum)
			{
                LOG("slave data\r\n");
                LOG("len:%d\r\n", pMsg->msg.handleValueNoti.len);
                for(uint8_t i=0; i<pMsg->msg.handleValueNoti.len; i++)
                {
                    LOG("%x ", pMsg->msg.handleValueNoti.value[i]);
                }
                LOG("\r\n");
//				ble_data_evt_t evt;
//            
//				evt.role = ROLE_MASTER;
//				evt.ev = BLE_EVT_BLE_DATA_RECIEVED;
//				evt.channel = index+1;
//				evt.param = pMsg->msg.handleValueNoti.len;
//                evt.data = pMsg->msg.handleValueNoti.value;
//                ble_data_evt(&evt);
			}
        }
    }
	else if( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP ||
		   ( (pMsg->method == ATT_ERROR_RSP) &&
             (pMsg->msg.errorRsp.reqOpcode == ATT_FIND_BY_TYPE_VALUE_REQ) ))
	{
		index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
		if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;

            LOG( "FindByTypeValueReq Error: %d,%d\r\n", index,status );
        }
        else
        {
            if(pMsg->msg.findByTypeValueRsp.numInfo > 0)
            {
                if(index < g_slaveMaxNum)
                {
                    slave_handle[index].uart_service_start = pMsg->msg.findByTypeValueRsp.handlesInfo[0].handle;
                    slave_handle[index].uart_service_end = pMsg->msg.findByTypeValueRsp.handlesInfo[0].grpEndHandle;
                }
            }
        }
        if ( ( pMsg->method == ATT_FIND_BY_TYPE_VALUE_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
            ( pMsg->method == ATT_ERROR_RSP ) )
        {
            //服务发现完成
            if(index < g_slaveMaxNum)
            {
                linkInitParam[index].link_status = LINK_STATUS_DISCOVER_ALL_CHARACTER;
                linkInitParam[index].link_cmd = 1;
                ble_task_set(USER_TASK_START, START_LINK_STATUS_EVT, NULL);
            }
        }
    }
    else if( pMsg->method == ATT_READ_BY_TYPE_RSP ||
           ( (pMsg->method == ATT_ERROR_RSP) &&
             (pMsg->msg.errorRsp.reqOpcode == ATT_READ_BY_TYPE_REQ) ))
    {
        index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
        if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;
            LOG( "ReadByTypeReq Error: %d,%d\r\n", index, status );
        }
        else
        {
            uint8 dataLen = pMsg->msg.readByTypeRsp.numPairs * pMsg->msg.readByTypeRsp.len;
            LOG("READ_BY_TYPE_RSP:%d \n",index);
            // A list of Attribute Handle and Attribute Value pairs is returned. Each
            // Attribute Value in the list is the Attribute Value for the characteristic
            // declaration. The Attribute Value contains the characteristic properties,
            // Characteristic Value Handle and characteristic UUID.
            for(uint8 i = 0; i < dataLen; i += pMsg->msg.readByTypeRsp.len)
            {
                // Check the Attribute Value for each Attribute Handle and Attribute
                // Value pairs for a matching characteristic UUID.
                LOG("masterLinkUUIDLen %d\n",masterLinkUUIDLen);
                if(ATT_CompareUUID(blemulti_RxCharUUID,masterLinkUUIDLen,
                                   &(pMsg->msg.readByTypeRsp.dataList[i+5]), pMsg->msg.readByTypeRsp.len-5))
                {
                    index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
                    if(index < g_slaveMaxNum)
                    {
                        slave_handle[index].write = pMsg->msg.readByTypeRsp.dataList[i+3] | (pMsg->msg.readByTypeRsp.dataList[i+4]<<8);
                    }
                }
                else if(ATT_CompareUUID(blemulti_TxCharUUID, masterLinkUUIDLen,
                                   &(pMsg->msg.readByTypeRsp.dataList[i+5]), pMsg->msg.readByTypeRsp.len-5))
                {
                    index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
                    if(index < g_slaveMaxNum)
                    {
                        slave_handle[index].notify = pMsg->msg.readByTypeRsp.dataList[i+3] | (pMsg->msg.readByTypeRsp.dataList[i+4]<<8);
                    }
                }
            } // for
        }
        if ( ( pMsg->method == ATT_READ_BY_TYPE_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
            ( pMsg->method == ATT_ERROR_RSP ) )
        {
            //特征发现完成
            if(index < g_slaveMaxNum)
            {
                linkInitParam[index].link_status = LINK_STATUS_DISCOVER_ALL_CHARACTER_DESC;
                linkInitParam[index].link_cmd = 1;
                ble_task_set(USER_TASK_START, START_LINK_STATUS_EVT, NULL);
            }
        }
    }
    else if( pMsg->method == ATT_FIND_INFO_RSP ||
           ( (pMsg->method == ATT_ERROR_RSP) &&
             (pMsg->msg.errorRsp.reqOpcode == ATT_FIND_INFO_REQ) ))
    {
        index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pMsg->connHandle);
        if(pMsg->method == ATT_ERROR_RSP)
        {
            uint8 status = pMsg->msg.errorRsp.errCode;
            LOG( "FindInfoReq Error:%d,%d\r\n", index, status );
        }
        else
        {
            //LOG("FindInfoRsp:%d\n",index);
            if(pMsg->msg.findInfoRsp.format == 1)	//16bit UUID
            {
                uint8 dataLen = pMsg->msg.findInfoRsp.numInfo * 4;
                uint8 numPairs = 0;
                for(uint8 i = 0; i < dataLen; i += 4)
                {
                    // Check the Attribute Value for each Attribute Handle and Attribute
                    // Value pairs for a matching characteristic UUID.
                    if(ATT_CompareUUID(clientCharCfgUUID, ATT_BT_UUID_SIZE,
                                       pMsg->msg.findInfoRsp.info.btPair[numPairs].uuid, ATT_BT_UUID_SIZE))
                    {
                        if(index < g_slaveMaxNum)
                        {
                            slave_handle[index].ccc = pMsg->msg.findInfoRsp.info.btPair[numPairs].handle;
                        }
                        break;
                    }
                    numPairs++;
                } // for
            }
        }
        if ( ( pMsg->method == ATT_FIND_INFO_RSP  && 
               pMsg->hdr.status == bleProcedureComplete ) ||
            ( pMsg->method == ATT_ERROR_RSP ) )
        {
            //CCC发现完成
            if(index < g_slaveMaxNum)
            {
                //使能notification
                linkInitParam[index].link_status = LINK_STATUS_ENABLE_NOTIFY;
                linkInitParam[index].link_cmd = 1;
                ble_task_set(USER_TASK_START, START_LINK_STATUS_EVT, NULL);
            }
        }
    }
}

static void simpleBLECentralStartConnecting(void)
{
	bStatus_t status;
	//if(g_masterConnectNum < MAX_CONNECTION_MASTER_NUM && calibrateMeterMode == 0)
	if(g_slaverConnectNum < g_slaveMaxNum)
	{
		if(osal_get_timeoutEx(simpleBLETaskId,MASTER_CONN_TIMEOUT_EVT) > 0)
		{
			LOG("Already Connecting\r\n");	
		}
		else
		{
            status = GAPMultiRole_EstablishLink(DEFAULT_LINK_HIGH_DUTY_CYCLE, TRUE, NULL, NULL);
			VOID osal_start_timerEx(simpleBLETaskId, MASTER_CONN_TIMEOUT_EVT, 5000);

            LOG("Connecting...%d %d\r\n",status,GAP_NumActiveConnections());
        }
    }
    else
    {
        LOG("All slave have been connected.\n");
    }
}

/*********************************************************************
 * @fn      simpleBLECentralRssiCB
 *
 * @brief   RSSI callback.
 *
 * @param   connHandle - connection handle
 * @param   rssi - RSSI
 *
 * @return  none
 */
void simpleBLECentralRssiCB(uint16 connHandle, int8 rssi)
{
    LOG("RSSI -dB: %d\r\n", (uint8)(-rssi));
}

/*********************************************************************
 * @fn      multiRoleAppEventCB
 *
 * @brief   Central event callback function.
 *
 * @param   pEvent - pointer to event structure
 *
 * @return  none
 */
#if CONNECT_CNT_TEST
uint32_t LinkCnt;
uint32_t noSyncCnt;
#endif
static void multiRoleAppEventCB(gapCentralRoleEvent_t *pEvent)
{
    //LOG("<<multiRoleAppEventCB: %d\n", pEvent->gap.opcode);
    
    switch(pEvent->gap.opcode)
    {
        case GAP_DEVICE_INIT_DONE_EVENT:
        {
            LOG("\r\nBLE Central Address: ");
            LOG(bdAddr2Str(pEvent->initDone.devAddr));
            LOG("\r\n");
        }
        break;

		case GAP_MAKE_DISCOVERABLE_DONE_EVENT:
			LOG("multiRoleAppEventCB ,GAP_MAKE_DISCOVERABLE_DONE_EVENT \n");
			break;

		case GAP_END_DISCOVERABLE_DONE_EVENT:
			LOG("multiRoleAppEventCB ,GAP_END_DISCOVERABLE_DONE_EVENT \n");
			break;

		case GAP_DEVICE_INFO_EVENT:
//            if(pEvent->deviceInfo.eventType != GAP_ADRPT_ADV_IND
//			   && pEvent->deviceInfo.eventType != GAP_ADRPT_ADV_DIRECT_IND)
//				break;

//			if(pEvent->deviceInfo.addrType == 0)
//			{
//                LOG("a\n");
//				LOG("mac:%02x%02x%02x%02x%02x%02x\r\n", pEvent->deviceInfo.addr[0], \
//														pEvent->deviceInfo.addr[1],
//														pEvent->deviceInfo.addr[2],
//														pEvent->deviceInfo.addr[3],
//														pEvent->deviceInfo.addr[4],
//														pEvent->deviceInfo.addr[5]);
//                for(uint8_t i=0; i<pEvent->deviceInfo.dataLen; i++)
//                {
//                    LOG("%x ", pEvent->deviceInfo.pEvtData[i]);
//                }
//                LOG("\r\n");
//                LOG("rssi:%d\r\n", pEvent->deviceInfo.rssi);
//            }
			break;

		case GAP_DEVICE_DISCOVERY_EVENT:
        {
            gapScanDevice_t *mac;
            gapScanDataEvent_t *pMsg = (gapScanDataEvent_t *)pEvent;

			LOG("Devices found finish:%d\n", pMsg->numDevs);
            
            for(uint8_t i=0; i<pMsg->numDevs; i++)
            {
                mac = pMsg->pDevList+i;
                LOG("mac:%02x%02x%02x%02x%02x%02x\r\n", mac->addr[0], \
                                        mac->addr[1],
                                        mac->addr[2],
                                        mac->addr[3],
                                        mac->addr[4],
                                        mac->addr[5]);
                LOG("AdData %d:", mac->AdData[0]);
                for(uint8_t j=0; j<mac->AdData[0]; j++)
                {
                    LOG("%02x ", mac->AdData[j+1]);
                }
                LOG("\r\n");

                LOG("ScanData %d:", mac->ScanData[0]);
                for(uint8_t j=0; j<mac->ScanData[0]; j++)
                {
                    LOG("%02x ", mac->ScanData[j+1]);
                }
                LOG("\r\n");
                
                LOG("rssi:%d\r\n", mac->rssi);
            };
        }
			break;

        case GAP_LINK_ESTABLISHED_EVENT:
        {
            uint8_t state;
            uint8_t index = 0xFF;
            uint8 initial_advertising_enable = TRUE;

            if(pEvent->gap.hdr.status == SUCCESS)
            {
//                hal_pwrmgr_lock(MOD_USR0);
                GAPMultiRole_GetParameter(GAPROLE_CONNECTION_STATE, &state, pEvent->linkCmpl.connectionHandle);

                if(state == GAPROLE_MASTER_CONNECTED_STATE)
                {
                    index = findConnectIndexByAddr(LL_ROLE_MASTER, pEvent->linkCmpl.devAddr);
                    //user_uart_task_set(USER_CMD_LINK_REP, index);
//					user_ch_open();
                    LOG("index %d\n",index);
                    if(index < g_slaveMaxNum)
                    {
                        slaveDevInfo[index].ConnectionHandle = pEvent->linkCmpl.connectionHandle;
                        slaveDevInfo[index].ConnectionState = 1;
                        //osal_memset(&slave_handle[index], 0, sizeof(slave_handle_t));
                        
                        linkInitParam[index].link_status = LINK_STATUS_READ_VERSION;
                        linkInitParam[index].link_cmd = 1;
                        ble_task_set(USER_TASK_START, START_LINK_STATUS_EVT, NULL);
                        LOG("slaveDevInfo[%d].ConnectionState %d",index,slaveDevInfo[index].ConnectionState);
                    }

                    g_slaverConnectNum++;
                    //g_slaveIndex |= (1<<index);
                    LOG("slave addr: 0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X \n",\
                        pEvent->linkCmpl.devAddr[0],pEvent->linkCmpl.devAddr[1],\
                        pEvent->linkCmpl.devAddr[2],pEvent->linkCmpl.devAddr[3],\
                        pEvent->linkCmpl.devAddr[4],pEvent->linkCmpl.devAddr[5]);
                    // stop cancel init timer
                    osal_stop_timerEx(simpleBLETaskId, MASTER_CONN_TIMEOUT_EVT);
                    //重启连接
                    LOG("after connect, start connecting \n");
                    ble_task_set(USER_TASK_START, START_MASTER_CONN_EVT, NULL);
                  
#if CONNECT_CNT_TEST					
                    if(index == 0)
                    {
                        LinkCnt++;
                        ble_task_set(USER_TASK_START, START_DISC_TEST_EVT, 1500);
                    }
#endif
                }
                else if(state == GAPROLE_SLAVE_CONNECTED_STATE)
                {
                    uint8_t i;
                    index = findConnectIndexByAddr(LL_ROLE_SLAVE, pEvent->linkCmpl.devAddr);
                    for(i=0; i<g_masterMaxNum; i++)
                    {
                        if(masterDevInfo[i].ConnectionState == 0)
                        {
                            masterDevInfo[i].ConnectionHandle = pEvent->linkCmpl.connectionHandle;
                            masterDevInfo[i].ConnectionState = 1;
                            masterDevInfo[i].PeerAddrType = pEvent->linkCmpl.devAddrType;
                            osal_memcpy(masterDevInfo[i].PeerAddr, pEvent->linkCmpl.devAddr, 6);
                            index = i;
                            break;
                        }
                    }
                    g_masterConnectNum++;
                    LOG("master addr: 0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X, type=%d \n",\
					    pEvent->linkCmpl.devAddr[0],pEvent->linkCmpl.devAddr[1],\
					    pEvent->linkCmpl.devAddr[2],pEvent->linkCmpl.devAddr[3],\
					    pEvent->linkCmpl.devAddr[4],pEvent->linkCmpl.devAddr[5],\
					    pEvent->linkCmpl.devAddrType);
				}
				
				LOG("Connect success. Index=%d, Handle = %d\n", index, pEvent->linkCmpl.connectionHandle);
				
				LOG("Connected OK.\n ");
			}
			else
			{
				LOG("Connect Failed. ");
				LOG("Reason: %x\r\n", pEvent->gap.hdr.status);
				if(LL_GetLinkRole(pEvent->linkCmpl.connectionHandle) == LL_ROLE_MASTER)
				{
					//连接失败后重启连接
					LOG("after connect failed, start connecting \n");
					ble_task_set(USER_TASK_START, START_MASTER_CONN_EVT, NULL);
				}
			}

			//after conect by master, advertising will stop, so need to restart advertising.
            if(g_masterConnectNum < g_masterMaxNum)
			{
				initial_advertising_enable = TRUE;
				state = GAPMultiRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8), &initial_advertising_enable, 0);
				LOG("Restart advertise:%d.\n", state);
			}
			else
			{
				initial_advertising_enable = FALSE;
				state = GAPMultiRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8), &initial_advertising_enable, 0);
			}
		}
		break;

		case GAP_LINK_TERMINATED_EVENT:
		{
			uint8_t state;
			uint8_t index;
			LOG("Link Terminate handle %d,reason:0x%02X \n",pEvent->linkTerminate.connectionHandle,pEvent->linkTerminate.reason);
			GAPMultiRole_GetParameter(GAPROLE_CONNECTION_STATE, &state, pEvent->linkTerminate.connectionHandle);
			
			if(state == GAPROLE_MASTER_DISCONNECTED_AFTER_TIMEOUT_STATE || state == GAPROLE_MASTER_DISCONNECTED_STATE)
			{
				index = findConnectIndexByConnectionHandle(LL_ROLE_MASTER, pEvent->linkTerminate.connectionHandle);
                g_slaverConnectNum--;
				LOG("master terminate index=%d\n",index);

                if(index < g_slaveMaxNum)
                {
                    if(slaveDevInfo[index].ConnectionActive)
                    {
//                        user_uart_task_set(USER_CMD_LINK_REP, index);
                    }
                    LOG("master addr: 0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,0x%02X,\n",\
                        slaveDevInfo[index].PeerAddr[0],slaveDevInfo[index].PeerAddr[1],\
                        slaveDevInfo[index].PeerAddr[2],slaveDevInfo[index].PeerAddr[3],\
                        slaveDevInfo[index].PeerAddr[4],slaveDevInfo[index].PeerAddr[5]\
                        );
                    slaveDevInfo[index].ConnectionHandle = 0xFF;
                    slaveDevInfo[index].ConnectionState = 0;
                    slaveDevInfo[index].ConnectionActive = 0;
                    
                    linkInitParam[index].link_status = LINK_STATUS_IDLE;
                    linkInitParam[index].link_cmd = 0;
                }
#if CONNECT_CNT_TEST
				if(index == 0)
				{
					if(pEvent->linkTerminate.reason == 0x3E)
					{
						noSyncCnt++;
					}
					LOG("Connect cnt=%d, noSyncCnt=%d\n", LinkCnt, noSyncCnt);
				}
#endif
#ifdef MODE_CALIB
				spi_send_cmd = SPI_CMD_DISC_CONN;
				user_spi_send_trig();
#endif
				//断开后重启连接
				LOG("after disconnect, start connecting \n");
				ble_task_set(USER_TASK_START, START_MASTER_CONN_EVT, 1000);
//				g_rfPhyTxPower = RF_PHY_TX_POWER_4DBM;//update tx power

                
            }
            else if(state == GAPROLE_SLAVE_DISCONNECTED_AFTER_TIMEOUT_STATE || state == GAPROLE_SLAVE_DISCONNECTED_STATE)
            {
                uint8 initial_advertising_enable = TRUE;
                index = findConnectIndexByConnectionHandle(LL_ROLE_SLAVE, pEvent->linkTerminate.connectionHandle);
                g_masterConnectNum--;
                //g_masterIndex &= ~(1<<index);
                LOG("slave terminate index=%d \n",index);

				if(index < g_masterMaxNum)
				{
					masterDevInfo[index].ConnectionHandle = 0xFF;
					masterDevInfo[index].ConnectionState = 0;
                    masterDevInfo[index].ConnectionActive = 0;
                    LOG("b:%d %d %d\r\n", masterDevInfo[index].ConnectionState, masterDevInfo[index].ConnectionHandle, g_masterConnectNum);
				}
                if(g_masterConnectNum < g_masterMaxNum)
				{
					//断开后重新开启广播
                    LOG("start adv\r\n");
					GAPMultiRole_SetParameter(GAPROLE_ADVERT_ENABLED, sizeof(uint8), &initial_advertising_enable, 0);
				}
//				g_rfPhyTxPower = RF_PHY_TX_POWER_4DBM;//update tx power
			}
            if( (g_masterConnectNum == 0) && (g_slaverConnectNum == 0) )
            {
//                hal_pwrmgr_unlock(MOD_USR0);
            }
		}
		break;

		case GAP_LINK_PARAM_UPDATE_EVENT:
		{
			LOG("Param Update\r\n");
		}
		break;

		default:
			break;
	}
}

/*********************************************************************
 * @fn      pairStateCB
 *
 * @brief   Pairing state callback.
 *
 * @return  none
 */
#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
static void simpleBLECentralPairStateCB(uint16 connHandle, uint8 state, uint8 status)
{
    if(state == GAPBOND_PAIRING_STATE_STARTED)
    {
//    LOG( "Pairing started" );
    }
    else if(state == GAPBOND_PAIRING_STATE_COMPLETE)
    {
        if(status == SUCCESS)
        {
//      LOG( "Pairing success" );
        }
        else
        {
//      LOG( "Pairing fail" );
        }
    }
    else if(state == GAPBOND_PAIRING_STATE_BONDED)
    {
        if(status == SUCCESS)
        {
//      LOG( "Bonding success" );
        }
    }
}
#endif
/*********************************************************************
 * @fn      simpleBLECentralPasscodeCB
 *
 * @brief   Passcode callback.
 *
 * @return  none
 */
#if (DEFAULT_GAPBOND_MGR_ENABLE==1)
static void simpleBLECentralPasscodeCB(uint8 *deviceAddr, uint16 connectionHandle,
                                       uint8 uiInputs, uint8 uiOutputs)
{
//    LOG("simpleBLECentralPasscodeCB\r\n");
#if (HAL_LCD == TRUE)

    uint32  passcode;
    uint8   str[7];

    // Create random passcode
    LL_Rand(((uint8 *) &passcode), sizeof(uint32));
    passcode %= 1000000;

    // Display passcode to user
    if(uiOutputs != 0)
    {
        LCD_WRITE_STRING("Passcode:",  HAL_LCD_LINE_1);
        LCD_WRITE_STRING((char *) _ltoa(passcode, str, 10),  HAL_LCD_LINE_2);
    }

    // Send passcode response
    GAPBondMgr_PasscodeRsp(connectionHandle, SUCCESS, passcode);
#endif
}
#endif
/*********************************************************************
 * @fn      multiRoleAppStartDiscoveryServiceByUUID
 *
 * @brief   Start service discovery by UUID.
 *
 * @return  none
 */
static bStatus_t multiRoleAppStartDiscoveryServiceByUUID(uint8_t masterIndex)
{	
    bStatus_t status;
    LOG("StartDisService: %d UUID len %d\r\n", masterIndex,masterLinkUUIDLen);
    //LOG("sizeof(blemulti_ServiceUUID)/sizeof(blemulti_ServiceUUID[0]) %d\n",sizeof(blemulti_ServiceUUID)/sizeof(blemulti_ServiceUUID[0]));
    //status = GATT_DiscPrimaryServiceByUUID(slaveDevInfo[masterIndex].ConnectionHandle, (uint8_t *)blemulti_ServiceUUID,sizeof(blemulti_ServiceUUID)/sizeof(blemulti_ServiceUUID[0]), simpleBLETaskId);
    status = GATT_DiscPrimaryServiceByUUID(slaveDevInfo[masterIndex].ConnectionHandle, (uint8_t *)blemulti_ServiceUUID,masterLinkUUIDLen, simpleBLETaskId);
    return status;
}

/*********************************************************************
 * @fn      multiRoleAppStartDiscoveryAllCharacteristic
 *
 * @brief   Start service discovery by UUID.
 *
 * @return  none
 */
static bStatus_t multiRoleAppStartDiscoveryAllCharacteristic(uint8_t masterIndex)
{	
    bStatus_t status;
    status = GATT_DiscAllChars(slaveDevInfo[masterIndex].ConnectionHandle, slave_handle[masterIndex].uart_service_start, slave_handle[masterIndex].uart_service_end, simpleBLETaskId);                             
    LOG("StartDisChar: %d,%d,%d,%d\r\n", masterIndex,slave_handle[masterIndex].uart_service_start,slave_handle[masterIndex].uart_service_end,status);
    return status;
}

/*********************************************************************
 * @fn      multiRoleAppStartDiscoveryAllCharacteristicDesc
 *
 * @brief   Start service discovery by UUID.
 *
 * @return  none
 */
static bStatus_t multiRoleAppStartDiscoveryAllCharacteristicDesc(uint8_t masterIndex)
{	
    bStatus_t status;
    LOG("StartDisCharDesc: %d\r\n", masterIndex);
    status = GATT_DiscAllCharDescs(slaveDevInfo[masterIndex].ConnectionHandle, slave_handle[masterIndex].notify+1, slave_handle[masterIndex].uart_service_end, simpleBLETaskId);    
    return status;	
}
/*********************************************************************
 * @fn      multiRoleAppEnableNotification
 *
 * @brief   Enable notification To BLE Server
 *
 * @return  none
 */

static bStatus_t multiRoleAppEnableNotification(uint8_t masterIndex)
{
    attWriteReq_t *pReq;
    bStatus_t status;
    
    pReq = osal_mem_alloc(sizeof(attWriteReq_t));
    pReq->sig = 0;
    pReq->cmd = 0;
    pReq->handle = slave_handle[masterIndex].ccc;
    pReq->len = 2;
    pReq->value[0] = (unsigned char)(GATT_CLIENT_CFG_NOTIFY);
    pReq->value[1] = (unsigned char)(GATT_CLIENT_CFG_NOTIFY >> 8);
    status = GATT_WriteCharDesc(slaveDevInfo[masterIndex].ConnectionHandle, pReq, simpleBLETaskId);
    LOG("Conn:%d\r\n", slaveDevInfo[masterIndex].ConnectionHandle);
    LOG("handle:%d\r\n", pReq->handle);
    if(status == SUCCESS)
    {
        LOG("Notify succ\r\n");
    }
    else
    {
        LOG("Notify ERROR %d\r\n", status);
    }
    osal_mem_free(pReq);
    return status;
}

uint16_t get_ble_tx_len_max(uint8_t role, uint8_t index)
{
    uint16 tx_max_len = 0;
    uint16 rx_max_len = 0;
    uint16 curr_mtu_size = 0;
    if(role == LL_ROLE_MASTER)
    {
        LL_ReadLocalDataLengh(slaveDevInfo[index].ConnectionHandle, &tx_max_len, &rx_max_len);
        curr_mtu_size = ATT_GetCurrentMTUSize(slaveDevInfo[index].ConnectionHandle);
    }
    else
    {
        LL_ReadLocalDataLengh(masterDevInfo[index].ConnectionHandle, &tx_max_len, &rx_max_len);
        curr_mtu_size = ATT_GetCurrentMTUSize(masterDevInfo[index].ConnectionHandle);
    }
    tx_max_len-=7;
    curr_mtu_size-=3;
    if(curr_mtu_size < (tx_max_len))
    {
        tx_max_len = curr_mtu_size;
    }
    return tx_max_len;
}

/*********************************************************************
 * @fn      user_data_send_by_ble
 *
 * @brief   调用时，需初始化msg中的value信息与len信息
 *
 * @return  
 */
bStatus_t user_data_send_by_ble(uint8_t role, uint8_t index, attMsg_t *msg)
{
    if((role == LL_ROLE_MASTER)&&(slaveDevInfo[index].ConnectionHandle != 0xff))
    {
        msg->writeReq.sig = FALSE;
        msg->writeReq.cmd = TRUE;
        msg->writeReq.handle = slave_handle[index].write;
        LOG("MB%d:%d\n", index, msg->writeReq.len);
        return GATT_WriteNoRsp(slaveDevInfo[index].ConnectionHandle, &msg->writeReq);
    }
    else
    {
        if ( bleuart_NotifyIsReady(masterDevInfo[index].ConnectionHandle) )
        {
            uint8 status;
            
            // Set the handle
            msg->handleValueNoti.handle = bleuart_ProfileAttrTbl[RAWPASS_TX_VALUE_HANDLE].handle;
            LOG("SB%d:%d\n", index+1, msg->handleValueNoti.len);
            // Send the Indication
            status = GATT_Notification( masterDevInfo[index].ConnectionHandle, &msg->handleValueNoti, FALSE);
            
            LOG("Notify:%d\r\n", status);
            return status;
        }
        else
        {
            return bleIncorrectMode;
        }
    }
}

/**
 * @brief user_data_to_slave
 * 
 */
#if 0
bStatus_t user_data_to_slave(uint8_t masterIndex, uint8 len, uint8 *pdata)
{
    attWriteReq_t *pReq;
    
    pReq = osal_mem_alloc(sizeof(attWriteReq_t));
    osal_memcpy(pReq->value, pdata, len);
    pReq->sig = FALSE;
    pReq->cmd = TRUE;
    pReq->handle = slave_handle[masterIndex].write;
    pReq->len = len;
    bStatus_t status = GATT_WriteNoRsp(slaveDevInfo[masterIndex].ConnectionHandle, pReq);
    LOG("handle:%d\r\n", pReq->handle);
    if(status == SUCCESS)
    {
        LOG("to_slave succ\r\n");
    }
    else
    {
        LOG("to_slave erro %d\r\n",status);
    }
    osal_mem_free(pReq);
    
    return status;
}
#endif
bStatus_t ble_link_dev(uint8_t *macAddr)
{
    return GAPMultiRole_EstablishLink(DEFAULT_LINK_HIGH_DUTY_CYCLE, FALSE, LL_DEV_ADDR_TYPE_PUBLIC, macAddr);
}

/*********************************************************************
 * @fn      simpleBLEFindSvcUuid
 *
 * @brief   Find a given UUID in an advertiser's service UUID list.
 *
 * @return  TRUE if service UUID found
 */
bool simpleBLEFindSvcUuid(uint16 uuid, uint8 *pData, uint8 dataLen)
{
    uint8 adLen;
    uint8 adType;
    uint8 *pEnd;

//    LOG("simpleBLEFindSvcUuid\r\n");

    pEnd = pData + dataLen - 1;

    // While end of data not reached
    while(pData < pEnd)
    {
        // Get length of next AD item
        adLen = *pData++;

//      LOG("==>adLen = %d ", adLen);
        if(adLen > 0)
        {
            adType = *pData;

//        LOG("adType = %d ", adType);
            // If AD type is for 16-bit service UUID
            if(adType == GAP_ADTYPE_16BIT_MORE || adType == GAP_ADTYPE_16BIT_COMPLETE)
            {
                pData++;
                adLen--;

                // For each UUID in list
                while(adLen >= 2 && pData < pEnd)
                {
                    // Check for match
                    if(pData[0] == LO_UINT16(uuid) && pData[1] == HI_UINT16(uuid))
                    {
                        // Match found
                        return TRUE;
                    }

                    // Go to next
                    pData += 2;
                    adLen -= 2;
                }

                // Handle possible erroneous extra byte in UUID list
                if(adLen == 1)
                {
                    pData++;
                }
            }
            else
            {
                // Go to next item
                pData += adLen;
            }
        }

        LOG("\r\n");
    }

    // Match not found
    return FALSE;
}


/*********************************************************************
 * @fn      bdAddr2Str
 *
 * @brief   Convert Bluetooth address to string
 *
 * @return  none
 */
char *bdAddr2Str(uint8 *pAddr)
{
    uint8       i;
    char        hex[] = "0123456789ABCDEF";
    static char str[B_ADDR_STR_LEN];
    char        *pStr = str;

    // Start from end of addr
    pAddr += B_ADDR_LEN;

    for(i = B_ADDR_LEN; i > 0; i--)
    {
        *pStr++ = hex[*--pAddr >> 4];
        *pStr++ = hex[*pAddr & 0x0F];
    }

    *pStr = 0;

    return str;
}
/*********************************************************************
 * @fn      bdAddr2Str
 *
 * @brief   Convert Bluetooth address to string
 *
 * @return  none
 */
uint8_t str_to_hex(const uint8_t ch)
{
    uint8_t value = 0;
    if(ch >= 0x30 && ch <=0x39)
    {
        value = ch - 0x30;
    }
    else if(ch >= 0x41 && ch <= 0x46)
    {
        value = ch - 0x37;
    }
    else if(ch >= 0x61 && ch <= 0x66)
    {
        value = ch - 0x57;
    }

    //LOG("value %02x\n",value);
    return value;
}
/**
 * @brief user_task_set
 * 
 */
void ble_task_set(uint8 action, uint16 event, uint32 time)
{
    osal_stop_timerEx(simpleBLETaskId, event);
    if(action == USER_TASK_START)
    {
        if(time)
        {
            osal_start_timerEx(simpleBLETaskId, event, time);
        }
        else
        {
            osal_set_event(simpleBLETaskId, event);
        }
    }
}

/*********************************************************************
*********************************************************************/
